import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

import serial
import time 

from threading import Lock
lock = Lock()

class TankDriver():
    def __init__(self):
        self.pub = rospy.Publisher('/odom', Odometry, queue_size=1)
        self.sub = rospy.Subscriber('/control_velocity', Twist, self.vel_callback)

        self.ser = serial.Serial('/dev/ttyUSB0', 115200)

    def send(self, cmd):
        print('send cmd: ', cmd)
        self.ser.write(cmd)
    
    def read_buffer(self):
        time.sleep(0.01)
        res = ''
        while self.ser.inWaiting() > 0:
            res += self.ser.read(1)
        res = bytearray(res)
        print('got res: ', res)
        return res
    def get_encoder(self):
        cmd = '?C\r'
        self.send(cmd)

    def set_speed(self, v1,v2):
        cmd = '!M {} {}\r'.format(v1, v2)
        self.send(cmd)

    def vel_callback(self, msg):
        print(msg.linear.x, msg.angular.z)
        lock.acquire()
        v1 = 200*msg.linear.x
        v1 += 100*msg.angular.z
        v2 = 200*msg.linear.x
        v2 -= 100*msg.angular.z
        self.set_speed(v1, -v2)
        self.read_buffer()
        lock.release()

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            lock.acquire()
            self.get_encoder()
            self.read_buffer()
            lock.release()

            #current_odom = Odometry()
            #self.pub.publish(current_odom)
            rate.sleep()

if __name__=='__main__':
    rospy.init_node('tank_driver001')
    tank_driver = TankDriver()
    tank_driver.set_speed(100,110)
    tank_driver.run()





